#include "uwb_dt/uwb_dt_publisher.hpp"

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/PoseArray.h"

#include <Eigen/Eigen>
#include <iostream>

void TestStringCallback(const std_msgs::String::ConstPtr& msg)
{
    int id;
    std_msgs::String receive;
    uwb_dt_publisher::unpackData(id, receive, msg);

    std::cout << "msg->data: " << receive.data << std::endl;
}

void TestPoseStampedCallback(const std_msgs::String::ConstPtr& msg)
{
    int id;
    geometry_msgs::PoseStamped receive;
    uwb_dt_publisher::unpackData(id, receive, msg);

    std::cout << "id: " << id << std::endl;
    std::cout << "receive.header.frame_id: " << receive.header.frame_id << std::endl;
    std::cout << "receive.header.stamp: " << receive.header.stamp << std::endl;
    std::cout << "receive.pose.position.x: " << receive.pose.position.x << std::endl;
    std::cout << "receive.pose.position.y: " << receive.pose.position.y << std::endl;
    std::cout << "receive.pose.position.z: " << receive.pose.position.z << std::endl;
    std::cout << "receive.pose.orientation.x: " << receive.pose.orientation.x << std::endl;
    std::cout << "receive.pose.orientation.y: " << receive.pose.orientation.y << std::endl;
    std::cout << "receive.pose.orientation.z: " << receive.pose.orientation.z << std::endl;
    std::cout << "receive.pose.orientation.w: " << receive.pose.orientation.w << std::endl;
}

void TestBoolCallback(const std_msgs::String::ConstPtr& msg)
{
    int id;
    std_msgs::Bool receive;
    uwb_dt_publisher::unpackData(id, receive, msg);

    std::cout << "receive.data: " << receive.data << std::endl;
}

void TestPoseArrayCallback(const std_msgs::String::ConstPtr& msg)
{
    int id;
    geometry_msgs::PoseArray receive;
    uwb_dt_publisher::unpackData(id, receive, msg);

    std::cout << "id: " << id << std::endl;
    std::cout << "receive.header.frame_id: " << receive.header.frame_id << std::endl;
    std::cout << "receive.header.stamp: " << receive.header.stamp << std::endl;
    std::cout << "receive.poses.size(): " << receive.poses.size() << std::endl;
    for (int i = 0; i < receive.poses.size(); i++)
    {
        std::cout << "receive.poses[" << i << "].position.x: " << receive.poses[i].position.x << std::endl;
        std::cout << "receive.poses[" << i << "].position.y: " << receive.poses[i].position.y << std::endl;
        std::cout << "receive.poses[" << i << "].position.z: " << receive.poses[i].position.z << std::endl;
        std::cout << "receive.poses[" << i << "].orientation.x: " << receive.poses[i].orientation.x << std::endl;
        std::cout << "receive.poses[" << i << "].orientation.y: " << receive.poses[i].orientation.y << std::endl;
        std::cout << "receive.poses[" << i << "].orientation.z: " << receive.poses[i].orientation.z << std::endl;
        std::cout << "receive.poses[" << i << "].orientation.w: " << receive.poses[i].orientation.w << std::endl;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "uwb_dt_test_node");
    ros::NodeHandle nh;

    ros::Subscriber sub_String = nh.subscribe("/test_String", 1, TestStringCallback);
    ros::Subscriber sub_PoseStamped = nh.subscribe("/test_PoseStamped", 1, TestPoseStampedCallback);
    ros::Subscriber sub_Bool = nh.subscribe("/test_Bool", 1, TestBoolCallback);
    ros::Subscriber sub_PoseArray = nh.subscribe("/test_PoseArray", 1, TestPoseArrayCallback);
    
    ros::spin();

    return 0;
}